/*
 * engineering_training_competition.c
 *
 *  Created on: Mar 27, 2024
 *      Author: boen
 */
#include <engineering_training_competition.h>

static uint16_t run_state = APP_NULL;

static uint32_t etc_tick = 0;

#define DEBUG_PICKUP			0
#define DEBUG_PLACE				0


void etc_timer(void)
{
	if(etc_tick>0)
	{
		etc_tick--;
	}
}


/**
* @brief
* @param
* @retval
*/
void etc_init(void)
{
	int error = 0;

	// 1:IMU,相机开始工作，底盘准备就绪
	etc_tick = 5000;
	while(1)
	{
		if((xbot.imu.state == SYS_FLAG_PLAY)&&
			(xbot.camera.state == SYS_FLAG_PLAY)&&
			(xbot.mobile.state == SYS_FLAG_IDLE)&&
			(xbot.manipulator.state == SYS_FLAG_IDLE)
		  )
		{
			break;
		}
		if(etc_tick == 0)
		{
			error = 1;
			break;
		}
	}

	// 2:机械臂归位
	etc_tick = 5000;
	while(1)
	{
		if(manipulator_set_joint(-1.5708, -1.5, 2.2, 1.08))
		//if(manipulator_set_joint(0.06f, 0.3f, -1.7f, -1.62f))
		{
			break;
		}
		if(etc_tick == 0)
		{
			error = 2;
			break;
		}
	}

	// 3:夹爪归位
	etc_tick = 5000;
	while(1)
	{
		if(manipulator_set_gripper(1))
		{
			break;
		}
		if(etc_tick == 0)
		{
			error = 3;
			break;
		}
	}

	// 4:托盘归位
	etc_tick = 5000;
	while(1)
	{
		if(manipulator_set_turntable(1))
		{
			break;
		}
		if(etc_tick == 0)
		{
			error = 4;
			break;
		}
	}
	printf("[app]: etc start. [%d]\r\n", error);

	xbot.run_task.c_point = 1;

#if	DEBUG_PICKUP
	run_state = 5;						// 测试抓取
#endif
#if DEBUG_PLACE
	run_state = 10;						// 测试放置
#endif
}


/**
* @brief
* @param
* @retval
*/
void etc_deinit(void)
{

}


/**
* @brief
* @param
* @retval
*/
int etc_obj_path(uint8_t obj_id, uint8_t axis)
{
	switch(obj_id)
	{
		case CAM_TARGET_RED:
			switch(xbot.run_task.c_point)
			{
				case 1:													//当前位置1-->目标位置1    不运动
					xbot.run_task.c_point = 1;
					return 1;
					break;
				case 2:													//当前位置2-->目标位置1    反向运动0.18米
					if(axis>0)
					{
						if(mobile_move_y(-0.18f))
						{
							xbot.run_task.c_point = 1;
							return 1;
						}
					}
					else
					{
						if(mobile_move_x(0.18f))
						{
							xbot.run_task.c_point = 1;
							return 1;
						}
					}
					break;
				case 3:													//当前位置3-->目标位置1    反向运动0.36米
					if(axis>0)
					{
						if(mobile_move_y(-0.36f))
						{
							xbot.run_task.c_point = 1;
							return 1;
						}
					}
					else
					{
						if(mobile_move_x(0.36f))
						{
							xbot.run_task.c_point = 1;
							return 1;
						}
					}
					break;
			}
			break;
		case CAM_TARGET_GREEN:
			switch(xbot.run_task.c_point)
			{
				case 1:													//当前位置1-->目标位置2    正向运动0.18米
					if(axis>0)
					{
						if(mobile_move_y(0.18f))
						{
							xbot.run_task.c_point = 2;
							return 1;
						}
					}
					else
					{
						if(mobile_move_x(-0.18f))
						{
							xbot.run_task.c_point = 2;
							return 1;
						}
					}
					break;
				case 2:													//当前位置2-->目标位置2    不运动
					xbot.run_task.c_point = 2;
					return 1;
					break;
				case 3:													//当前位置3-->目标位置2    反向运动0.18米
					if(axis>0)
					{
						if(mobile_move_y(-0.18f))
						{
							xbot.run_task.c_point = 2;
							return 1;
						}
					}
					else
					{
						if(mobile_move_x(0.18f))
						{
							xbot.run_task.c_point = 2;
							return 1;
						}
					}
					break;
			}
			break;
		case CAM_TARGET_BLUE:
			switch(xbot.run_task.c_point)
			{
				case 1:													//当前位置1-->目标位置3    正向运动0.36米
					if(axis>0)
					{
						if(mobile_move_y(0.36f))
						{
							xbot.run_task.c_point =3;
							return 1;
						}
					}
					else
					{
						if(mobile_move_x(-0.36f))
						{
							xbot.run_task.c_point = 3;
							return 1;
						}
					}
					break;
				case 2:													//当前位置2-->目标位置3    正向运动0.18米
					if(axis>0)
					{
						if(mobile_move_y(0.18f))
						{
							xbot.run_task.c_point = 3;
							return 1;
						}
					}
					else
					{
						if(mobile_move_x(-0.18f))
						{
							xbot.run_task.c_point = 3;
							return 1;
						}
					}
					break;
				case 3:													//当前位置3-->目标位置3    无运动
					xbot.run_task.c_point = 3;
					return 1;
					break;
			}
			break;
		default:
			break;
	}
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int etc_obj_align(uint8_t mode, float data[4])
{
	static int step = 0;
	int maxX = 0;
	int minX = 0;
	int maxY = 0;
	int minY = 0;
	static float tmp = 0;
	if(mode)
	{
		maxX = CAM_PICKUP_X_MAX;
		minX = CAM_PICKUP_X_MIN;

		maxY = CAM_PICKUP_Y_MAX;
		minY = CAM_PICKUP_Y_MIN;
	}
	else
	{
		maxX = CAM_PLACE_X_MAX;
		minX = CAM_PLACE_X_MIN;

		maxY = CAM_PLACE_Y_MAX;
		minY = CAM_PLACE_Y_MIN;
	}
	switch(step)
	{
		case 0:
			tmp = 0;
			if((xbot.camera.color.target_y >= minY)&&
				(xbot.camera.color.target_y <= maxY)
			)
			{
				step = 2;
			}
			else
			{
				if(xbot.camera.color.target_y <= minY)
				{
					// 2cm->40pix    deltaX = 0.02/41 = 0.0005
					tmp = 0.0005f*(minY - xbot.camera.color.target_y);
				}
				else if(xbot.camera.color.target_y >= maxY)
				{
					// 4cm->40pix    deltaX = 0.04/40 = 0.001
					tmp = -0.001f*(xbot.camera.color.target_y - maxY);
				}
				step = 1;
			}
			break;
		case 1:
			if(mode)
			{
				if(mobile_move_x(tmp))
				{
					step +=1;
				}
			}
			else
			{
				if(mobile_move_y(tmp))
				{
					step +=1;
				}
			}
			break;
		case 2:
			if(etc_tick == 0)
			{
				if((xbot.camera.color.target_x >= minX)&&
					(xbot.camera.color.target_x <= maxX)
				)
				{
					step = 0;
					return 1;
				}
				else
				{
					// 当前角度+主要移动距离+次要移动补偿（防止计算出的主要距离不能收敛到拾取范围）
					// 弧度变化和像素变化的比例系数 -- 通过物理测量得到采样参数
					// 0.0018 = 0.11(rad)/62(pix)
					if(xbot.camera.color.target_x != 0)
					{
						if(xbot.camera.color.target_x <= minX)
						{
							data[0] = xbot.manipulator.servo[PHY_SERVO_1].fActPOS
									+ 0.0018f*(minX - xbot.camera.color.target_x)
									+ 0.025f;
						}
						else if(xbot.camera.color.target_x >= maxX)
						{
							data[0] = xbot.manipulator.servo[PHY_SERVO_1].fActPOS
									- 0.0018f*(xbot.camera.color.target_x - maxX)
									- 0.025f;
						}
						step+=1;
					}
				}
			}
			break;
		case 3:
			if(manipulator_set_joint(data[0], 0.50f, 1.1f, 1.58f))
			{
				etc_tick = 1000;
				step = 2;
			}
			break;
		default:
			break;
	}
	return 0;
}


/**
* @brief   车辆和拾取目标的距离需要为20CM左右（通过调整车辆运动来得到）
* @param
* @retval
*/
int etc_pickup(uint8_t obj_id)
{
	static int step = 0;
	static float data[4];
	static float tmp = 0;
	switch(step)
	{
	case 0:																// 根据任务自动搜寻物体
		if(etc_obj_path(obj_id, 1))
		{
			data[0] = 0;
			data[1] = 0;
			data[2] = 0;
			data[3] = 0;
			tmp = 0;
			if(manipulator_set_turntable(obj_id))
			{
				step+=1;
			}
		}
		break;
	case 1:																// 设置机械臂抓取初始姿态
		if(manipulator_set_joint(0.0f, 0.50f, 1.1f, 1.58f))
		{
			step+=1;
		}
		break;
	case 2:																// 设置相机参数
		camera_set_target(obj_id);
		camera_set_func(APPLICATION_COLORDETECT);
		etc_tick = 1000;
		step+=1;
		break;
	case 3:																// 机械臂末端对齐待抓物体
		if(etc_tick==0)
		{
			if(etc_obj_align(1, data))
			{
				step+=1;
			}
		}
		break;
	case 4:																// 机械臂收缩下探放置碰到物体
		if(manipulator_set_joint(data[0], 0.0f, 2.0f, -1.57f))
		{
			step+=1;
		}
		break;
	case 5:																// 机械臂到达物体前方
		// 机械臂精度误差 0.06f
		if(data[0]>0)
		{
			tmp = tan(data[0]+0.06)*0.118f;
		}
		else if(data[0]<0)
		{
			tmp = tan(data[0]-0.06)*0.118f;
		}
		else
		{
			tmp = 0.0f;
		}

		if(manipulator_set_pose(0.118f, tmp, -0.05f, 0.0f, 0.0f, 0.0f))
		{
			step+=1;
		}
		break;
	case 6:																// 机械臂到达物体位置
		// 机械臂精度误差 0.06f
		if(data[0]>0)
		{
			tmp = tan(data[0]+0.06)*0.192f;
		}
		else if(data[0]<0)
		{
			tmp = tan(data[0]-0.06)*0.192f;
		}
		else
		{
			tmp = 0.0f;
		}

		if(manipulator_set_pose(0.192f, tmp, -0.05f, 0.0f, 0.0f, 0.0f))
		{
			step+=1;
		}
		break;
	case 7:																// 闭合夹爪
		if(manipulator_set_gripper(0))
		{
			step+=1;
		}
		break;
	case 8:																// 将物体放入车后支架上
		if(manipulator_set_joint(0.06f, 0.3f, -1.7f, -1.62f))
		{
			step+=1;
		}
	case 9:																// 松开夹爪
		if(manipulator_set_gripper(1))
		{
			step+=1;
		}
		break;
	case 10:															// 复位机械臂姿态
		if(manipulator_set_joint(0.0f, 0.50f, 1.1f, 1.58f))
		{
			step=0;
			return 1;
		}
		break;
	default:
		break;
	}
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int etc_place(uint8_t obj_id)
{
	static int step = 0;
	static float data[4];
	float tmp_x = 0;
	float tmp_y = 0;
	switch(step)
	{
		case 0:
			if(etc_obj_path(obj_id, 0))
			{
				if(manipulator_set_turntable(obj_id))
				{
					step+=1;
				}
			}
			break;
		case 1:
			if(manipulator_set_joint(1.5708, 0.50, 1.0599, 1.5708))
			{
				step+=1;
			}
			break;
		case 2:
			camera_set_target(obj_id);
			camera_set_func(APPLICATION_COLORDETECT);
			etc_tick = 2000;
			step+=1;
			break;
		case 3:
			if(etc_tick == 0)
			{
				if(etc_obj_align(0, data))
				{
					step+=1;
				}
			}
			break;
		case 4:
			if(manipulator_set_joint(0, 0.25, -1.7417, -1.55))
			{
				step+=1;
			}
			break;
		case 5:
			if(manipulator_set_gripper(0))
			{
				step+=1;
			}
			break;
		case 6:
			if(manipulator_set_joint(0, 0.8, -1.7417, -1.55))
			{
				step+=1;
			}
			break;
		case 7:
			// 基准末端位置 0.0f, 0.21f, 0.0f, 需要保证tan(X)中的X不能为90°，防止计算错误
			if((data[0]>-0.006)&&(data[0]<0.006))
			{
				tmp_x = 0;
			}
			else
			{
				tmp_x = 0.21f*tan(1.5708-data[0]);
			}
			tmp_y = 0.21f;
			//tmp_y = 0.21/cos(fabs(1.5708-data[0]));
			if(manipulator_set_pose(tmp_x, tmp_y, -0.04f, 0.0f, 0.0f, 0.0f))
			{
				step+=1;
			}
			break;
		case 8:
			if(manipulator_set_gripper(1))
			{
				step+=1;
			}
			break;
		case 9:
			if(mobile_move_y(-0.01))
			{
				step +=1;
			}
			break;
		case 10:
			if(manipulator_set_joint(1.5708f, 0.6542f, 2.1667f, -1.2292f))
			{
				step=0;
				return 1;
			}
			break;
		default:
			break;
	}
	return 0;
}


/**
* @brief
* @param
* @retval
*/
void etc_polling(void)
{
	if(xbot.state == SYS_FLAG_PLAY)
	{
		switch(run_state)
		{
			case 0:
				if(mobile_move_y(0.15f))												// 沿着Y正方向从站点移出
				{
					run_state+=1;
				}
				break;
			case 1:
				if(mobile_move_x(0.5f))													// 沿着X正方向到达任务（二维码）获取点
				{
					run_state+=1;
				}
				break;
			case 2:
				if(((xbot.camera.qrcode.t1_H > 0)&&										// 确认任务一的第一个对象ID
					(xbot.camera.qrcode.t1_M > 0)&&										// 确认任务一的第二个对象ID
					(xbot.camera.qrcode.t1_L > 0))&&									// 确认任务一的第三个对象ID
					((xbot.camera.qrcode.t2_H > 0)&&									// 确认任务二的第一个对象ID
					(xbot.camera.qrcode.t2_M > 0)&&										// 确认任务二的第二个对象ID
					(xbot.camera.qrcode.t2_L > 0))										// 确认任务二的第三个对象ID
					)
				{
					xbot.run_task.c_task = 1;
					run_state+=1;
				}
				break;
			case 3:
				if(mobile_move_x(1.3f))												// 沿着X正方向到达地图右上转弯处
				{
					run_state+=1;
				}
				break;
			case 4:
				if(mobile_move_y(0.6f))													// 沿着Y正方向到达目标拾取点
				{
					xbot.run_task.c_point = 1;
					run_state+=1;
				}
				break;
			case 5:
#if DEBUG_PICKUP
				xbot.run_task.c_point = 1;
				if(etc_pickup(1))
				{
					run_state = 100;
				}
#else
				if(etc_pickup(xbot.camera.qrcode.t1_H))									// 拾取任务中的第一个物体
				{
					run_state+=1;
				}
#endif
				break;
			case 6:
				if(etc_pickup(xbot.camera.qrcode.t1_M))									// 拾取任务中的第二个物体
				{
					run_state+=1;
				}
				break;
			case 7:
				if(etc_pickup(xbot.camera.qrcode.t1_L))									// 拾取任务中的第三个物体
				{
					if(etc_obj_path(CAM_TARGET_BLUE, 1))
					{
						run_state+=1;
					}
				}
				break;
			case 8:
				if(mobile_move_y(0.56f))												// 沿着Y正方向到达地图左上转弯处
				{
					run_state+=1;
				}
				break;
			case 9:
				if(mobile_move_x(-0.68f))												// 沿着X负方向到达目标放置点
				{
					xbot.run_task.c_point = 1;
					run_state+=1;
				}
				break;
			case 10:
#if DEBUG_PLACE
				xbot.run_task.c_point = 1;
				if(etc_place(1))
				{
					run_state = 100;
				}
#else
				if(etc_place(xbot.camera.qrcode.t1_H))									// 放置任务中的第一个物体
				{
					run_state+=1;
				}
#endif
				break;
			case 11:
				if(etc_place(xbot.camera.qrcode.t1_M))									// 放置任务中的第二个物体
				{
					run_state+=1;
				}
				break;
			case 12:
				if(etc_place(xbot.camera.qrcode.t1_L))									// 放置任务中的第三个物体
				{
					if(etc_obj_path(CAM_TARGET_BLUE, 0))
					{
						xbot.camera.qrcode.t1_H = 0;
						xbot.camera.qrcode.t1_M = 0;
						xbot.camera.qrcode.t1_L = 0;
						run_state+=1;
					}
				}
				break;
			case 13:
				if(mobile_move_x(-0.70f))												// 沿着X负方向到达地图左下转弯处
				{
					run_state+=1;
				}
				break;
			case 14:
				if(mobile_move_y(-1.78f))												// 沿着Y负方向到达地图右下转弯处
				{
					run_state+=1;
				}
				break;
			default:
				xbot.state = SYS_FLAG_IDLE;
#if	DEBUG_PICKUP
				run_state = 5;						// 测试抓取
#elif DEBUG_PLACE
				run_state = 10;						// 测试放置
#else
				run_state = 0;
#endif

				break;
		}
	}
}
